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The first-order two-step nonlinear estimator, when applied to a problem of orbital 
navigation, is found to occasionally produce first step covariance matrices with very 
low eigenvalues at certain trajectory points. This anomaly is the result of the linear 
approximation to the first step covariance propagation. The study of this anomaly 
begins with expressing the propagation of the first and second step covariance matrices 
in terms of a single matrix. This matrix is shown to have a rank equal to the 
difference between the number of first step states and the number of second step 
states. Furthermore, under some simplifying assumptions, it is found that the basis 
of the column space of this matrix remains fixed once the filter has removed the 
large initial state error. A test matrix containing the basis of this column space 
and the partial derivative matrix relating first and second step states is derived. 
This square test matrix, which has dimensions equal to the number of first step 
states, numerically drops rank at the same locations that the first step covariance 
does. It is formulated in terms of a set of constant vectors (the basis) and a matrix 
which can be computed from a reference trajectory (the partial derivative matrix). 
A simple example problem involving dynamics which are described by two states and 
a range measurement illustrate the cause of this anomaly and the application of the 
aforementioned numerical test in more detail. 


INTRODUCTION 

The two-step optimal estimator derived in Haupt, et al. [1] and Kasdin, et al. [2] provides an 
improved recursive solution to the state estimation problem involving nonlinear measurements. This 
method breaks the problem into two parts by defining a set of first step states which permit an exact 
linear measurement model and a nonlinear relationship between the desired states and the first step 
states. In attempting to apply this first order version of this method to the problem of navigating 
two satellites relative to each other in an elliptical orbit it is found that occasionally a covariance 
matrix for the first step states with one very small eigenvalue results. This makes the second step 
state update ill-conditioned and potentially causes subsequent filter divergence or meaningless state 
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estimates. This paper presents an analysis of the cause of this problem and derives a numerical test 
for the location of the ill-conditioned first step state covariances. 

A covariance matrix with very low or zero eigenvalues indicates that a strong correlation has 
developed between the states and that some linear combination of the states can be known almost 
exactly. The problems for which the two step filter was developed, however, are non-linear. For a 
nonlinear function relating the first and second step states there would not exist, in general, a linear 
combination of first step states which could be known exactly. The analysis presented in this paper is 
interpreted as identifying a problem with the linear approximations used for the first step covariance 
propagation in the filter, not to imply that it is possible to find some linear combination of first step 
states which truly have zero error. 


TWO-STEP ESTIMATOR OVERVIEW 


Given a non-linear system described by a state vector, x, of dimension m and the differential equation 
x = g(x, £), the general estimation problem is to process discrete vector measurements, i^, of dimension 
l related to the state through the nonlinear function zl = h(xi,ti) and generate the best estimate of 
the state at those points, x^. Haupt, et al. [1] define a “first step” state vector, y , of dimension n (for 
the cases considered here n > m) related to x, the “second step” state, through a nonlinear function 
yl = f(xi,ti). & is chosen such that the measurement vector can be expressed as a linear combination 
of the first step states zl = Hyl. (In all cases considered in this study H is constant.) Thus each new 
measurement is incorporated through a linear system where traditional methods of linear estimation 
are applicable. 

Mechanization of the two step filter is given in the references [1] and [2] as follows: 1) Perform 
a standard linear measurement update of the first step state estimate and covariance based on the 
observations. 2) Compute the second step state estimates by a nonlinear estimation process such as the 
Gauss-Newton or Lavenberg-Marquardt [4] methods and update the second step covariance matrix by 
a first order approximation. 3) Propagate the second step states and covariance matrix forward to the 
time of the next measurement using the state transition matrix or numerical integration as required. 
4) Propagate the first step states and covariance matrix to the time of the next measurement using 
the a posteriori first and second step covariance matrices from the last measurement update and the 
propagated ( a priori ) second step covariance matrix for the upcoming measurement epoch. 

This last step is the focus of the research presented here. Equation (33) from reference [1] 
approximates the first step covariance propagation from the i TH time step to the i + 1 st step to first 
order. 
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In which a hat Q over the state vector indicates an estimate of that vector and the notation of (+) 
signifies the a posteriori conditions and (-) signifies the a priori conditions. 

This generates the a priori covariance matrix at the i + 1 st time step by adjusting the a 
posteriori covariance from the i TH step by the difference between two positive semidefinite matrices. 
It is not clear that Eq. (1) is guaranteed to produce a non-singular and positive-definite covariance 
matrix, owing to the negative sign on the third term. In fact, when applying the two-step filter to an 
orbital mechanics problem, this equation occasionally resulted in covariance matrices with very low 
or negative eigenvalues. The following analysis seeks to understand the conditions under which the 
two step filter would produce such an ill-conditioned first step covariance matrix and also to derive a 
test to predict when this condition will occur. As the smallest eigenvalues would have to go through 
zero before becoming negative, this is also a test for non-positive-definite matrices in addition to rank 
deficient matrices. 

The combination of Eqs. (25) and (19) from reference [1] gives. 
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This expression represents the update of the second step covariance from the (post-measurement 
updated) first step covariance following the iterative update of the of the second step state (step 
number 2 above). 

It should be noted that in actual practice, both for the orbital mechanics problem which was 
the motivation for this work and in the simple numerical example presented later in this paper, a 
UDU t factored form of the two step filter is used [2]. The original two-step filter from [1] will be used 
in the analysis in this paper because it is mathematically identical but simpler to understand than 
the factored form. The specific UDU T factored algorithm used in all of the numerical simulations in 
given in reference [2] and the Appendix. 


ANALYSIS OF THE PROBLEM 

An analysis into the cause of ill-conditioned first step covariances can be broken into three steps. 
First, the filter equations are expressed in terms of a new matrix, (7, which is a function of the two 
state covariance matrices, P x and P y . Second, two properties are found for the column space of C 
which allow this space to be represented by a reduced number of constant basis vectors. Finally, a 
numerical test is derived using the set of basis vectors and the reference or predicted state trajectory 
which identifies points in which P y may become ill-conditioned. 


Formulation in Terms of the Matrix C 


The covariance propagation equations for P x and P y in the two step estimator described in the previous 
section are expressed in terms of another matrix, (7, defined at each time step for the a priori and a 
posteriori conditions. 


Ci{±) = P yi {±) 


d_l 

dx 


p Xi (±) 


Xi(±) 


d£ 

dx 


Xi(±) 


(3) 


Equation (1) reduces to 


Ci+ 1(-) — c*(+) 


(4) 


Next, two assumptions are made about the evolution of the first and second step covariance 
matrices once any large initial state error has been removed. 

Assumption 1: P“ 1 (— ) is approximated as: 


^ ( ’ ~ dx 
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This equation is found by taking Eq. (2), defined for the a posteriori conditions, and applying 
it the the aposteriori conditions. 

Assumption 2: The partial derivative matrix is approximately constant across 
measurement updates. 


d£ 

* d J. 

„ d J. 

dx 
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Computing this partial matrix from the true, reference, or predicted state are all assumed to be 
equivalent. The state estimate is of interest in this analysis only as it enters the two-step estimator 
covariance prediction through the partial derivative matrix, df /dx. Henceforth, no reference will be 
made to which side of the measurement update the partial matrix is computed on. 

These two assumptions, taken together, amount to approximating the second step covariance 
matrix propagation along a nominal trajectory as equal to that of a linearized Kalman filter which 
processes measurements and directly updates the second step states. This is shown as follows; The 
first step covariance update in the two step filter is given by 


Py~ 1 (+)=Py~ 1 (-)+H T R7 1 H 


(7) 
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Premultiply this n by n matrix equation by (df /dx) \J and postmultiplied it by (df/dx)\i. Then 
use (5) and (6) to reduce the resulting m by m matrix expression to the form of a second step state 
covariance update 

P- 1 (+) = P~ l (-) + Hi RT x H Xi (8) 

The measurement matrix is H Xi = H(df / dx)\{. Equation (8) is recognized as the covariance update 
in an linearized Kalman filter processing measurements zl and updating the second step state directly 
from them. The propagation of the second step states between measurements would be the same for 
the two step filter as well as the linearized filter. Therefore, these two assumptions can be interpreted 
as the assumption that the evolution of the covariance matrices in two step filter is the same as that 
for a linearized Kalman filter applied to the same problem. This was found to be true once the large 
initial state errors were reduced. 


Properties of the C Matrix 


Here, two important properties of the C matrix which greatly simplify its propagation and ultimate 
use in the test for ill-conditioned P y , are derived. Although conditions under which P y becomes ill- 
conditioned are of interest here, it is necessary to assume that the matrix P~ x exists. These two 
points are reconciled by interpreting this analysis as an inquiry into where P y becomes numerically 
lower rank, assuming that it will always have an inverse analytically. 

First Property: The Rank of C is n — m. This property is obtained by considering Eq. (3) 
not as a definition for Ci(+) but as an expression for P yi (+) given a known Ci(+) and P Xi (+) 


p„( + ) = a(+)+f 


P (+) 

Xi[+) dx 


(9) 


and noting that the term 


91 

dx 
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Xi[+) dx 


(10) 


is of rank m (assuming P Xi (+) is full rank). In order for P yi (+) to be full rank, Ci(+) must contain 
at least n — m column vectors which are linearly independent of the column vectors of (10). Hence, 
the requirement 

rank(Ci(+)) > n — m (11) 


Post-multiplying Eq. (3) by the matrix 1 (- b) 
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and using Eq. (2) results in 


Ci(+)P 3/i 1 (+) 


d_l 

dx 


= 0 


(13) 


Equation (13) indicates that the m columns of Py^ (+)d f / dx\i are in the nullspace of Ci(+). 
If the first step states are independent of each other at each time step, then the matrix (df /dx\ i ) is 
of rank m. This would always be true in the suggested practice [1] of defining the first step states as 
the set of second step states augmented by the nonlinear measurement equations. In this case m rows 
of the partial derivative matrix form an m by m identity. 

The rank of the matrix product Py^ (+)d f / dx\i is therefore less than or equal to m (P“ 1 (- b) 
must be full rank ). If the rank of this product was less than m, however, then there would exist some 
vector a / 0 such that P~ x (+)<9 / / dx\ia = 0. Post-multiplying the partial derivative matrix, which 
must have m linearly independent columns, by a nonzero vector, a, will give another non-zero vector, 
b ; (df /dx)\ia — b^ 0. This results in the contradiction; T > “ 1 (+)6 = 0. Therefore, the matrix product 
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1 (+)9//9x|i must have a rank equal to m. It is therefore concluded that Ci(+) also has at least 
m linearly independent null vectors and hence the dimension of its nullspace is at least m. 

null(Ci(+)) > m (14) 

Combining Eqs. (11) and (14) and using the fundamental relationship for any n by n matrix 

rank(Ci(+)) + null(Ci(+)) = n (15) 


results in the conclusion 

rank{Ci{+)) = n — m (16) 

and, consequently 

null(Ci(+)) = m (17) 

From Eq. (4), the nullspace and column space of Ci+i {— ) are identical to those same spaces 
defined for Ci(+). This rank property would consequently apply to the a priori condition as well. 

rank(Ci+i (— )) = n — m (18) 


Second Property: The Column Space of C is Fixed in R n for a steady state filter with l < m. 
Consider that the nullspace and row space of a matrix are orthogonal complements, ie. , every vector in 
one must be orthogonal to every vector in the other. Therefore, if it can be shown that the nullspace 
of C remains fixed, then the row space of C must also be fixed. The row space and column space of 
C are equivalent because it is a symmetric matrix (defined as the sum of two symmetric matrices). 
This analysis will therefore focus on showing that the nullspace of C remains constant. 

To simplify the notation, the matrices A^(+) and A^(— ) will be defined for the products 


Ni(±) 


P ~ 1 (± ) -t 

Vi K > dx 


(19) 


The statement that the nullspace of C remains the same between measurement updates (Eq. 
(4)) is equivalent to the statement that the columns of A^ + i(— ) span the same space as the columns 
of Ni(+). This can be written as 

iV i+1 (-)=iV i (+)A i+1 (20) 


in which Ai+i is a set of coefficients expressing each column vector of A^ + i(— ) as a linear combination 
of the column vectors of A^(+). 

The update of the first step covariance at the i + 1 st time step using the standard Kalman 

filter is 


P -1 

Vi + i 


(+)=Py< + 1 (~)+H 1 R£ 1 H 


( 21 ) 


Post-multiplying by (df/dx)\i+i, expressing this in terms of the A^ + i(±) matrix defined in (19) and 
substituting in (20) results in 


N i+ 1 ( + ) 


Ni(+)Ai + i + H T R~^ 


H d -l 

H dx 


i + 1 


( 22 ) 


First, consider the special case in which the columns of H T lie in the column space of A^(+). 
The columns of H T can then be used as l of the basis vectors for the column space of A^(+). The 
other basis vectors are defined as the columns of an n by m — l matrix N. The matrix N{(+) can 
therefore be expressed as the linear combination 


Ni(+) = NBi + H T Di 


(23) 


Substituting this into (22) gives 


N i+1 (+) = NBiA i+1 + H t 


df 

DiA i+ i + R^H— 


*+l 


(24) 
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Therefore the same basis {TV, H T } spans the column space of both TV^ + 1 (+) and iV*(+) and conse- 
quently spans the nullspace of both C*+i(+) and C*(+). This shows that if l < m and H T is in the 
nullspace of C then the column space of C remains fixed in R n . 

The arguments given above can be extended heuristically to the situations in which H T does 
not initially lie in the column space of TVf(+). As each new measurement that is processed, a new 
term of the form H T R~ 1 H(df / dx) is added to iV*(+). The column vectors of this term, which are 
in the column space of # T , are the only modification that the filter can make to the nullspace of C. 
The only direction in which the columns of TV can be changed is within the span of H T . One would 
expect that as i increases, some l dimensional subspace of the column space of TVf(±) would approach 
the column space of H T . This, of course, does not rigorously prove this property. The numerical 
simulations, however, run on models with l < m all have produced a C with a column space that 
stays fixed after the initial state transients are reduced. 

In the special case in which l = m and the filter is started with little a priori state knowledge 
(. P ~ very small), the matrix TV 0 (+) could be approximated as 


No(+) 


T U - 1 


H 1 R I 


Of 

h r l 

dx 0 


(25) 


for the initial time steps because the magnitudes of the column vectors of are much larger 

than those of the columns of P“ 1 (+). For this special case, the columns of H T would span the 
nullspace of C. 

In the case of l > n, the same arguments used above can be used to show that the columns of 
H t are the only modifications which are possible to the column space of (7, but there are too many 
column vectors in H T to form a basis. For this reason, it will not generally be true that the column 
space of C stays fixed for l > m. The span of the columns of the matrix product 


H T R^H 


d_l 

dx . 


(26) 


does not stay constant because of the partial derivative factor. The basis of the nullspace of C would 
be determined by the accumulation of terms like (26) from all previous time steps. This case would 
be rare in actual practice. Most estimation problems do not involve an observation vector which has 
a larger dimension than the state vector. 

It is only the space spanned by the columns of C which is of interest, not the actual elements 
of the C matrix. This space has been demonstrated to remain fixed in R n when l < m. For the 
remainder of this paper we will use the notation {ci, <? 2 , to indicate the basis for the column 

space of C without any reference to a specific time step or a priori or a posteriori state. 

It should be emphasized that these results are independent of how the second step state co- 
variance is propagated between measurements. Including process noise would not effect the possibility 
of generating the ill-conditioned covariance matrices. It may, however, change the specific location of 
these anomalies by generating a different C matrix. 


Test for Ill-conditioned First Step Covariance 

The C matrix is now used to derive a test to predict the location of points in which the P y matrix may 
become ill-conditioned. It must be assumed that P~ 1 still exists and that the filter will never actually 
generate a singular P y . The test which is desired will be one which sets a numerical tolerance on how 
close to singular we allow P y to become. Also, it is desired to have a test which is independent of 
the filter operations so that it could be operated on a reference or predicted trajectory to determine 
beforehand which sections of the trajectory are susceptible to the ill-conditioned covariances. Such a 
rank test can be derived by starting with (9). 

As mentioned before, Eq. (9) expresses a matrix which must be of rank n as the sum of 
a matrix of rank n — m and one of rank m. This would be possible as long as the n — m ba- 
sis vectors {c{, c 2 , . . . c n _ m } are linearly independent of the column space of ( df/dx)P x {df/dx ) T . 
This has already been shown when deriving the first property of C . We now consider cases where 
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at least one basis vector of C becomes close to being linearly dependent on the column space 
of {d f / dx)P x {d f / dx) T . To simplify this further and remove the time-dependent and filter-state- 
dependent covariance P x , consider that the column space of {d f / dx)P x {d f / dx) T is spanned by the 
m columns vectors {df /dx i, df /dx 2, . . . df /dx m }. This leads to a test to look for conditions under 
which the set of n — m vectors {ci, C2, . . . c n _ m } become close to being linearly dependent upon the set 
of m column vectors {df /dx 1, df /dx 2, . . . df /dx m }. (From the assumption in Eq. ( 6 ) these partial 
derivatives can be computed from a reference trajectory or from a state trajectory predicted ahead of 
the current filter time). 

All of this can be summarized in a test of the numerical rank test of an n by n matrix. 

,e) < n ( 27 ) 

for some tolerance e. Points for which the condition described in Eq. ( 27 ) is true are the locations where 
an ill-conditioned first step covariance matrix would occur. The numerical rank of an n by n matrix 
is defined as the number of singular values greater than e [ 3 ]. The span of {ci, C2, ...c n -™} is fixed 
(property 2 ) and the partial derivative matrix is computed along a reference trajectory (assumption 
1 ). Therefore, the test defined in ( 27 ) could conceivably be applied to points ahead of the present filter 
state to identify future trajectory points in which generating an ill-conditioned covariance matrix is 
possible. 

Geometric Interpretation 

These concepts are visualized geometrically using a system of two second step states and three first 
step states (n = 3 and m — 2 ) as shown in Figure 1 . The covariance matrix P x is represented by the 
two-dimensional error ellipse in the x\ — x 2 plane. The effect of ( 10 ) is to rotate and scale that ellipse 
to a new orientation in the 2/1 — 2/2 — 2/3 space. The column space of df /dx defines the plane of that 
ellipse. The first property of the C matrix is that its rank is 1 , hence its column space is a single line 
in the 2/1 — 2/2 — 2/3 space as shown in Figure 1 . 

The second property of C states that the orientation of this line would remain fixed in 2/1 — 2/2 — 
2/3 space. The plane defined by the column space of {df /dx), however, would change orientation with 
the state vector evolution. This line and plane together span R 3 under non-degenerate conditions. 
The orientation of the plane with respect to the line control the linear independence of the columns 
of the P y matrix as described by Eq. ( 27 ). 

As long as this line is not coplanar with the column space of {df /dx) P y remains rank 3 . If 
this line does fall in the plane defined by the column space then the union of these two subspaces 
would not span R 3 and consequently P y would be singular. If the line lies nearly in the plane, then 
the P y matrix is ill-conditioned. In this case, there will exist a coordinate transformation such that in 
one direction in the 2/1 — 2/2 — 2/3 space the linear covariance approximation given in ( 1 ) would predict 
that the first step state estimate will have very little error. This transformation will define a linear 
combination of the over-determined first step states which the filter predicts can be known almost 
exactly. In reality, because the first and second step states are nonlinearly related, there cannot be a 
linear combination of first step states which are known exactly. 


rank 


df_ df_ 
dx 1 ’ dx 2 


df 

dx rn 


NUMERICAL EXAMPLE 


A simple two state example problem is used to illustrate this anomaly in the two step filter. The 
geometry of this problem is shown in Figure 2 . The kinematics consist of a particle following a spiral 
path defined by a constant angular velocity uo 0 and a constant radial velocity i?o as if the particle was 
attached to a string of increasing length. The nonlinear differential equations in Cartesian coordinates 
are 


dx 1 
dt 


XlVp 

\Af +%l 


- x 2 ojo 


( 28 ) 
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Figure 1: Geometric Interpretation of the Column Space of P 




Figure 2: Example Problem Geometry 


dx 2 

dt 
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The linearized state dynamics matrix, A(x), used to propagate the state transition matrix by 
numerically integrating $ = A(x)§ is 


A = 


x 2 vq 

(x\+xl)% 


_ V^^X^ X2 

(x 2 +x 2 )i 
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(x 2 +X 2 ) 2 
X^Vp 


- 


(*?+*! 


(30) 


The measurement is the range from a fixed point located at the coordinates (1,0) to the 
particle. This gives the measurement equation. 


z = 


Xi - l ) 2 + x\ 


(31) 


The first step states are defined as the second step states augmented by the measurement, following 
the suggestion in reference [1]. 


y = 


y/( x 1 - l) 2 + ^2 

Xi 


(32) 


X-2 


Hence H = [ 1 0 0 ] and the partial matrix, df /dx, is given by; 


dj_ 

dx 


X\—l 

yj (x 1 -l) 2 +xl 

1 

0 


X “2 

yj (Xi—1) 2 +X 2 

0 

1 


(33) 


Specific numbers used in the example and the filter implementations are all listed in table 
1. The initial conditions are in error from the reference starting conditions (2,0) and a normally 
distributed error is added to the measurements. A process noise term is included in the filter to 
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Table 1: 


NUMERICAL DATA FOR EXAMPLE PROBLEM 


Number of Filtered Points 
Independent Variable (t) range 
Filter Initial State (x G (+)) 
True Initial State (x(0)) 
Measurement Noise (lcr, normal distribution) 
Measurement Covariance ( R ) 
Filter a priori Second Step State Covariance (P c o(~ )) 


30001 
0 : 6 
{ 2 . 0 , 0 . 0 } 
{2.583,0.313} 
0.01 
io - 4 

diag{ 0.25,0.25} 


Filter a priori First Step State Covariance (P y o (— )): 


0.226 0.209 0.0 

0.209 0.247 1.85 x 10“ 4 

0.0 1.85 x 10- 4 0.248 


Second Step State Discrete Time Process Noise ( Qd ): diag{ 10 12 10 12 } 


prevent the filter from losing sensitivity to new measurements as t oo and P y ^ x 0. No dynamic 
noise was simulated in the truth model, however. A plot of the particle motion and one set of noisy 
measurements is shown in Figure 3. A large number (30000) of data points were generated, this was 
to illustrate the sharp decreases in eigenvalues of P y which can occur only over a very small time 
period. 

Eigenvalues of the P y (+) matrix on the order of 10 -15 occurred near t « 1.3 and t « 4.8. The 
matrix C is computed from each point in the filter time history from the a posteriori state covariances 
and estimated state. The largest eigenvector of C was used as the basis of the column space of C. 
Figure 4 plots the numerical values demonstrating the aforementioned properties of C . The three 
singular values of C are plotted in Figure 4(a). Note that the first singular values on the order of 
10 18 larger than the other two, and consequently the rank of C is 1 (property 1). The dot product 
between the “steady state” c, computed as the average of the last 5000 points, and each previous c\ is 
plotted in Figure 4(b). This dot product is very nearly unity once the large initial state error has been 
reduced by the filter. Hence, it has been demonstrated that the column space of C is fixed (property 
2 )- 

The explanation given in this study for the constant basis of C’s column space is illustrated 
in Figure 4(c). In this figure the dot product between the H matrix and the basis vector c is plotted. 
After t « 0.5 this product becomes very small in comparison to the magnitude of the H T and c vectors 
(both unity) indicating that the two vectors are orthogonal and therefore the column of H T is in the 
nullspace of C . The small jump near t « 1.3 in Figure 4(c) is possibly caused by numerical problems 
resulting from the near singularity at that point. The magnitude of this deviation is still very small 
as compared to unity. 

Figure 5 illustrates the rank test defined in Eq. (27) using a numerical tolerance of 10 -4 
on top of a plot of the eigenvalues of P y (+). This test condition is computed from the steady state 
eigenvector of C and the partial derivative matrix computed along the true trajectory. The rank test 
correctly predicts the two locations of low eigenvalues in the P y time history as illustrated on Figure 

5. 

Figure 6 plots the square of the error between the filter a posteriori prediction for the first 
step states and the true values of those states, rotated into the direction of the minimum eigenvalue of 
P y . On top of that plot is the minimum eigenvalue of P y from the filter predicted first step covariance. 
All of the numerical results presented herein were from a single filter run. Monte Carlo simulations 
of a statistically distributed ensemble of runs would produce similar results. The specific locations of 
the points in which P y becomes ill-conditioned, however, would be slightly different for each member 
of the ensemble because of differences in the orientation of the space {ci, <? 2 , ...c n _ m } depending upon 
the starting conditions and the (noisy) measurement time history. 
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Figure 5: Numerical Rank Test for Ill-Conditioned P y 
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Eigenvalue decompositon of Py is Py = V D V 
Error vector is computed as: e = y - y TRUE 



Figure 6: First Step State Error in Minimum eig(P y ) Direction 





CONCLUSION 


The occurrence of ill-conditioned time updates for the first step covariance matrix in the two step 
filter has been explained. This anomaly is the result of the linearized approximation to the first step 
covariance propagation. In this approximation, there were conditions under which the positive definite 
second step covariance matrices did not combine to produce a full set of linearly independent column 
vectors for the first step covariance matrix. This analysis was simplified by the ability to formulate the 
filter covariance propagation in terms of a reduced set of basis vectors which, under certain conditions 
and assumptions, spanned a fixed space. One result of this study was a numerical test of the linear 
dependence of those basis vectors and the partial derivative matrix which was shown to correspond 
with the first step covariance matrix becoming ill conditioned. 

APPENDIX 


UDU t Factored Algorithm for the Two Step Filter [2], [5] 


• Initialization of the following: 

^c 0 ( + ), ^2/o ( + )? x ~o( + ); 2/o( + ) = /(r~o( + )) (34) 

• t Propagation of the second step states and the state transition matrix by numerical 

integration from the i — 1 st to the i TH time step. The first step state is estimated from 


Vi(-) = Vi- 1(+) + f(xi(-)) ~ f(xi- 1(+)) 


(35) 


The second step covariance is propagated by computing the block matrices Y x and D x 


Y* = 


4b,i— 1 U Xi _! ( + ) 


G d 


(36) 


D Xi _ l (+) : 0 

0 i Q d 


(37) 


and then reducing them to a m by m upper triangular and diagonal matrices, U Xi (—) and 
D Xi (—), respectively, such that 


U Xi {-)D Xi {-)D T Xi {~) = Y x D x Yj = P Xi (~) 


(38) 


This is done using the Modified Weighted Gram-Schmidt (MWGS) [5] method. 

Similarly, the subsequent propagation of the first step states as defined in Eq. (1) is done by 
computing the matrices Y y and D y . 


Yy = 


TJ ( + ) • 77 (-) ■ 

u Vi-i\^J ■ dx „ , , u Xi\ ) ■ dx 

Xi(-) 


Dy — 


Dy i _ 1 (+) : 


0 


: D Xi {~) i 


Xi-l{ + ) 

0 

0 


U i _ 1 (+) 


0:0: —D Xi _ 1 (+) J 

The MWGS process is then used to compute U Vi (—) and D yi ( — ) such that 


U yi (-)D yi (-)Ul(-) =YyD y Y v T 


(39) 


(40) 


(41) 
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• The measurement update is performed by converting the observation vector Z{ into l uncorrelated 
scalar observations (z*) with unity variance through a coordinate transformation. 


WW T = Ri 

(42) 

r = w _1 f 

(43) 

H* = W _1 H 

(44) 


Details of the scalar measurement update are given in [5]. This step will result in the a posteriori 
first step states (&(+))and U yi (+) and D yi (+) factors. The inverse of P“ 1 (+) can then be 
computed by the more numerically stable method 


Pv i 1 (+) = U~ T (+) 


l/D yi (+)(l,l) 


l/D yi (+)(2,2) 


1 / D vi(+)(n,n) J 


Uy i \+) (45) 


• The a posteriori second step states are found from the U Vi (+) and D yi (+) by numerically 

minimizing the cost function 

m = \m+) - m) T Py i H+)M+) - m) ( 46 ) 


The Lavenberg-Marquardt method [4] is used for this numerical minimization. 

+ The iterations are initialized with x° = Xi(-) and A = 0.001. At each k TH iteration, the 
Hessian is computed from the approximation 


H g 


dj_ 

dx 


p~ i (+) — 

Vi 1 J dx 


The linear system 


[a]Sx = — 


dJ{x) 


dx 


is solved to update the state estimate 


X k+1 =x k +6x 


(47) 

(48) 

(49) 


in which the gradient of J is 


dJ_ 

dx 


& k 


{Vi ~ f{xk))P yi 1 {+) 


and a is computed from 


c^i, i = Ho t l (1 + A) 
OL P ,q = H Gpq for p^q 


(50) 

(51) 

(52) 


The change in the cost function J(x k+1 ) determines how the next iteration is performed. 

- If J(x k+1 ) > J(x k ) then A = 10A; 

k = k + 1; x k+1 = x k and execution is returned to J. 

— If J(x k+1 ) < J(x k ) then A = A/ 10; k = k + 1; and execution is returned to J. 


The iterations are continued until the cost function decreases by less than 0.01. After this 
indicates convergence, the a posteriori second step states are updated by 

Xi(+) = x k (53) 

and the a posteriori second step covariance is computed from Eq. (2). U Xi (+) and D Xi (+) are 
then obtained by Cholesky factorization. The time step is incremented i = i + 1 and execution 
is returned to t 
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